#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/WrenchStamped.h>
#include <sensor_msgs/Illuminance.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Range.h>
#include <signal.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/UInt8MultiArray.h>
#include <webots_ros/BoolStamped.h>
#include <webots_ros/Float64Stamped.h>
#include <webots_ros/Int32Stamped.h>
#include <webots_ros/Int8Stamped.h>
#include <webots_ros/RadarTarget.h>
#include <webots_ros/RecognitionObject.h>
#include <webots_ros/RecognitionObjects.h>
#include <webots_ros/StringStamped.h>
#include <cstdlib>
#include "ros/ros.h"

#include <webots_ros/get_bool.h>
#include <webots_ros/get_float.h>
#include <webots_ros/get_float_array.h>
#include <webots_ros/get_int.h>
#include <webots_ros/get_string.h>
#include <webots_ros/get_uint64.h>
#include <webots_ros/get_urdf.h>
#include <webots_ros/set_bool.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_float_array.h>
#include <webots_ros/set_int.h>
#include <webots_ros/set_string.h>

#include <webots_ros/camera_get_focus_info.h>
#include <webots_ros/camera_get_info.h>
#include <webots_ros/camera_get_zoom_info.h>
#include <webots_ros/display_draw_line.h>
#include <webots_ros/display_draw_oval.h>
#include <webots_ros/display_draw_pixel.h>
#include <webots_ros/display_draw_polygon.h>
#include <webots_ros/display_draw_rectangle.h>
#include <webots_ros/display_draw_text.h>
#include <webots_ros/display_get_info.h>
#include <webots_ros/display_image_copy.h>
#include <webots_ros/display_image_delete.h>
#include <webots_ros/display_image_load.h>
#include <webots_ros/display_image_new.h>
#include <webots_ros/display_image_paste.h>
#include <webots_ros/display_image_save.h>
#include <webots_ros/display_set_font.h>
#include <webots_ros/field_disable_sf_tracking.h>
#include <webots_ros/field_enable_sf_tracking.h>
#include <webots_ros/field_get_bool.h>
#include <webots_ros/field_get_color.h>
#include <webots_ros/field_get_count.h>
#include <webots_ros/field_get_float.h>
#include <webots_ros/field_get_int32.h>
#include <webots_ros/field_get_name.h>
#include <webots_ros/field_get_node.h>
#include <webots_ros/field_get_rotation.h>
#include <webots_ros/field_get_string.h>
#include <webots_ros/field_get_type.h>
#include <webots_ros/field_get_vec2f.h>
#include <webots_ros/field_get_vec3f.h>
#include <webots_ros/field_import_node.h>
#include <webots_ros/field_remove.h>
#include <webots_ros/field_set_bool.h>
#include <webots_ros/field_set_color.h>
#include <webots_ros/field_set_float.h>
#include <webots_ros/field_set_int32.h>
#include <webots_ros/field_set_rotation.h>
#include <webots_ros/field_set_string.h>
#include <webots_ros/field_set_vec2f.h>
#include <webots_ros/field_set_vec3f.h>
#include <webots_ros/lidar_get_frequency_info.h>
#include <webots_ros/lidar_get_info.h>
#include <webots_ros/motor_set_control_pid.h>
#include <webots_ros/node_add_force_or_torque.h>
#include <webots_ros/node_add_force_with_offset.h>
#include <webots_ros/node_disable_contact_points_tracking.h>
#include <webots_ros/node_disable_pose_tracking.h>
#include <webots_ros/node_enable_contact_points_tracking.h>
#include <webots_ros/node_enable_pose_tracking.h>
#include <webots_ros/node_get_center_of_mass.h>
#include <webots_ros/node_get_contact_points.h>
#include <webots_ros/node_get_field.h>
#include <webots_ros/node_get_field_by_index.h>
#include <webots_ros/node_get_id.h>
#include <webots_ros/node_get_name.h>
#include <webots_ros/node_get_number_of_fields.h>
#include <webots_ros/node_get_orientation.h>
#include <webots_ros/node_get_parent_node.h>
#include <webots_ros/node_get_pose.h>
#include <webots_ros/node_get_position.h>
#include <webots_ros/node_get_static_balance.h>
#include <webots_ros/node_get_status.h>
#include <webots_ros/node_get_string.h>
#include <webots_ros/node_get_type.h>
#include <webots_ros/node_get_velocity.h>
#include <webots_ros/node_remove.h>
#include <webots_ros/node_reset_functions.h>
#include <webots_ros/node_set_joint_position.h>
#include <webots_ros/node_set_string.h>
#include <webots_ros/node_set_velocity.h>
#include <webots_ros/node_set_visibility.h>
#include <webots_ros/pen_set_ink_color.h>
#include <webots_ros/range_finder_get_info.h>
#include <webots_ros/receiver_get_emitter_direction.h>
#include <webots_ros/robot_get_device_list.h>
#include <webots_ros/robot_set_mode.h>
#include <webots_ros/robot_wait_for_user_input_event.h>
#include <webots_ros/save_image.h>
#include <webots_ros/skin_get_bone_name.h>
#include <webots_ros/skin_get_bone_orientation.h>
#include <webots_ros/skin_get_bone_position.h>
#include <webots_ros/skin_set_bone_orientation.h>
#include <webots_ros/skin_set_bone_position.h>
#include <webots_ros/speaker_play_sound.h>
#include <webots_ros/speaker_speak.h>
#include <webots_ros/supervisor_get_from_def.h>
#include <webots_ros/supervisor_get_from_id.h>
#include <webots_ros/supervisor_get_from_string.h>
#include <webots_ros/supervisor_movie_start_recording.h>
#include <webots_ros/supervisor_set_label.h>
#include <webots_ros/supervisor_virtual_reality_headset_get_orientation.h>
#include <webots_ros/supervisor_virtual_reality_headset_get_position.h>

#define TIME_STEP 5

using namespace std;

static int model_count;
static vector<string> model_list;
static vector<unsigned char> imageColor;
static vector<float> imageRangeFinder;
static int connectorPresence = 0;
static double accelerometerValues[3] = {0, 0, 0};
static double compassValues[3] = {0, 0, 0};
static double GPSValues[3] = {0, 0, 0};
static double GPSSpeedVectorValues[3] = {0, 0, 0};
static double GyroValues[3] = {0, 0, 0};
static double inertialUnitValues[4] = {0, 0, 0, 0};
static double touchSensorValues[3] = {0, 0, 0};
static bool callbackCalled = false;
double odom_x,odom_y,odom_theta;

string model_name = "my_robot";
ros::ServiceClient time_step_client;
webots_ros::set_int time_step_srv;

static bool doubleIsEqual(double value, double expected, double delta) {
  return ((isnan(value) && isnan(expected)) || (isinf(value) && isinf(expected)) || (fabs((value) - (expected)) <= (delta)));
}

void modelNameCallback(const std_msgs::String::ConstPtr &name) {
  model_count++;
  model_list.push_back(name->data);
  ROS_INFO("Model #%d: %s.", model_count, model_list.back().c_str());
  callbackCalled = true;
}


void rangeFinderCallback(const sensor_msgs::Image::ConstPtr &image) {
  int size = image->width * image->height;
  imageRangeFinder.resize(size);

  const float *depth_data = reinterpret_cast<const float *>(&image->data[0]);
  for (int i = 0; i < size; ++i)
    imageRangeFinder[i] = depth_data[i];
  callbackCalled = true;
}


void lidarCallback(const sensor_msgs::Image::ConstPtr &image) {
  callbackCalled = true;
}


bool callbackCalled1,callbackCalled2,callbackCalled3;
void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values) {
  GPSValues[0] = values->point.x;
  GPSValues[1] = values->point.y;
  GPSValues[2] = values->point.z;

  ROS_INFO("GPS values are x=%f y=%f z=%f (time: %d:%d).", GPSValues[0], GPSValues[1], GPSValues[2], values->header.stamp.sec,
   values->header.stamp.nsec);
  callbackCalled1 = true;
}

void GPSSpeedCallback(const webots_ros::Float64Stamped::ConstPtr &value) {
  ROS_INFO("GPS speed is: %fkm/h (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec);
  callbackCalled2 = true;
}

void GPSSpeedVectorCallback(const geometry_msgs::PointStamped::ConstPtr &values) {
  GPSSpeedVectorValues[0] = values->point.x;
  GPSSpeedVectorValues[1] = values->point.y;
  GPSSpeedVectorValues[2] = values->point.z;

  ROS_INFO("GPS speed vector values are x=%f y=%f z=%f (time: %d:%d).", GPSSpeedVectorValues[0], GPSSpeedVectorValues[1],
   GPSSpeedVectorValues[2], values->header.stamp.sec, values->header.stamp.nsec);
  callbackCalled3 = true;
}




void pre_treatment(ros::NodeHandle &n)
{
  ros::Subscriber name_sub = n.subscribe("model_name", 100, modelNameCallback);
  while (model_count == 0 || model_count < name_sub.getNumPublishers()) {
    ros::spinOnce();
    ros::spinOnce();
    ros::spinOnce();
  }
  ros::spinOnce();
  name_sub.shutdown();

  //begin main loop

  string model;
  string path;
  string data;
  vector<string> device_list;

  //get_model

  if (time_step_client.call(time_step_srv) && time_step_srv.response.success)
    ROS_INFO("time_step service works.");
  else
    ROS_ERROR("Failed to call service time_step to update robot's time step.");

  //get_number_of_devices_client
  ros::ServiceClient get_number_of_devices_client =
    n.serviceClient<webots_ros::get_int>(model_name + "/robot/get_number_of_devices");
  webots_ros::get_int get_number_of_devices_srv;

  if (get_number_of_devices_client.call(get_number_of_devices_srv)) {
    int number_of_devices = get_number_of_devices_srv.response.value;
    ROS_INFO("%s has %d devices.", model_name.c_str(), number_of_devices);
  } else
    ROS_ERROR("Failed to call service get_number_of_devices.");

  get_number_of_devices_client.shutdown();
  time_step_client.call(time_step_srv);

  //get_device_list
  ros::ServiceClient device_list_client =
    n.serviceClient<webots_ros::robot_get_device_list>(model_name + "/robot/get_device_list");
  webots_ros::robot_get_device_list device_list_srv;

  if (device_list_client.call(device_list_srv)) {
    device_list = device_list_srv.response.list;
    for (unsigned int i = 0; i < device_list.size(); i++)
      ROS_INFO("Device [%d]: %s.", i, device_list[i].c_str());
  } else
    ROS_ERROR("Failed to call service device_list.");

  device_list_client.shutdown();
  time_step_client.call(time_step_srv);
}

void get_gps(ros::NodeHandle &n,string name)
{

  ros::Subscriber sub_GPS_32;
  // ros::Subscriber sub_GPS_speed;
  // ros::Subscriber sub_GPS_speed_vector;

  sub_GPS_32 = n.subscribe(model_name + "/"+name+"/values", 1, GPSCallback);
  // sub_GPS_speed = n.subscribe(model_name + "/"+name+"/speed", 1, GPSSpeedCallback); 
  // sub_GPS_speed_vector = n.subscribe(model_name + "/"+name+"/speed_vector", 1, GPSSpeedVectorCallback);
  
  callbackCalled1 = false;
  // callbackCalled2 = false;
  // callbackCalled3 = false;
  while ((sub_GPS_32.getNumPublishers() == 0 || !callbackCalled1)
  // &&(sub_GPS_speed.getNumPublishers() == 0 || !callbackCalled2)
  // &&(sub_GPS_speed_vector.getNumPublishers() == 0 || !callbackCalled3)
  ) {
    ros::spinOnce();
    // ROS_INFO("%d",sub_GPS_32.getNumPublishers());
    // time_step_client.call(time_step_srv);
  }
  sub_GPS_32.shutdown();

  // sub_GPS_speed = n.subscribe(model_name + "/"+name+"/speed", 1, GPSSpeedCallback); 
  // sub_GPS_speed_vector = n.subscribe(model_name + "/"+name+"/speed_vector", 1, GPSSpeedVectorCallback);
  
  // callbackCalled = false;
  // while (sub_GPS_speed.getNumPublishers() == 0 || !callbackCalled) {
  //   ros::spinOnce();
  //   // time_step_client.call(time_step_srv);
  // }
  // sub_GPS_speed.shutdown();
 
  // sub_GPS_speed_vector = n.subscribe(model_name + "/"+name+"/speed_vector", 1, GPSSpeedVectorCallback);
  // callbackCalled = false;
  // while (sub_GPS_speed_vector.getNumPublishers() == 0 || !callbackCalled) {
  //   ros::spinOnce();
  //   // time_step_client.call(time_step_srv);
  // }
  // sub_GPS_speed_vector.shutdown();

}

void set_motor(ros::NodeHandle &n,string &name,float speed,float position)
{
  ros::ServiceClient set_velocity_client;
  set_velocity_client = n.serviceClient<webots_ros::set_float>(model_name + "/"+name+"/set_velocity");

  webots_ros::set_float set_velocity_srv;
  set_velocity_srv.request.value = speed;
  if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success)
    ROS_INFO("Velocity set %f",speed);
  else
    ROS_ERROR("Failed to call service set_velocity on motor.");

  set_velocity_client.shutdown();
  // time_step_client.call(time_step_srv);

  ros::ServiceClient set_position_client;
  set_position_client = n.serviceClient<webots_ros::set_float>(model_name + "/"+name+"/set_position");

  webots_ros::set_float set_position_srv;
  set_position_srv.request.value = position;
  if (set_position_client.call(set_position_srv) && set_position_srv.response.success)
    ROS_INFO("Position set %f",position);
  else
    ROS_ERROR("Failed to call service set_position on motor.");

  set_position_client.shutdown();
  // time_step_client.call(time_step_srv);
}

void updata_odom(const std_msgs::Float64MultiArray::ConstPtr & odom)
{

  ROS_INFO("odom is: x:%f y:%f theta:%f", odom->data[0], odom->data[1], odom->data[2]);
  odom_x=odom->data[0]; odom_y=odom->data[1];odom_theta=odom->data[2];
  callbackCalled=true;
}

// void get_lidar(ros::NodeHandle &n,string name)
// {

// }

void quit(int sig) {
  time_step_srv.request.value = 0;
  time_step_client.call(time_step_srv);
  ROS_INFO("User stopped the node.");
  ros::shutdown();
  exit(0);
}
int main(int argc,char **argv)
{
  ////////////////////////////////////////////////////////////////
  //pre_treatment
  ////////////////////////////////////////////////////////////////
  ros::init(argc,argv,"test1");
  ros::NodeHandle n;
  signal(SIGINT, quit);

  time_step_client = n.serviceClient<webots_ros::set_int>(model_name + "/robot/time_step");
  time_step_srv.request.value = TIME_STEP;
  pre_treatment(n);
  
  string motor1="motor1";
  string motor2="motor2";
  set_motor(n,motor1,5, M_PI/6);
  set_motor(n,motor2,4,100000);
  
  ros::Subscriber odom;

  odom=n.subscribe<std_msgs::Float64MultiArray>(model_name+"/odom",10,updata_odom);
  
  // ////////////////////////////////////////////////////////////////////////
  // //GPS
  // ////////////////////////////////////////////////////////////////////////
  // ros::ServiceClient set_GPS_client1,set_GPS_client2;
  // webots_ros::set_int GPS_srv;
 
  // set_GPS_client1 = n.serviceClient<webots_ros::set_int>(model_name + "/gps1/enable");
  // set_GPS_client2 = n.serviceClient<webots_ros::set_int>(model_name + "/gps2/enable");
  // string gps1="gps1";
  // string gps2="gps2";

  // GPS_srv.request.value = TIME_STEP;
  // if (set_GPS_client1.call(GPS_srv) && GPS_srv.response.success && set_GPS_client2.call(GPS_srv) && GPS_srv.response.success) {
  //   ROS_INFO("GPS enabled.");
  // } 
  // else {
  //   if (!GPS_srv.response.success)
  //     ROS_ERROR("Sampling period is not valid.");
  //   ROS_ERROR("Failed to enable GPS.");
  //   return 1;
  // }
  // while(true)
  // {
  //   get_gps(n,gps1);
  //   // get_gps(n,gps2);
  //   ros::spinOnce();
  // }
  
  
  // ////////////////////////////////////////////////////////////////////////
  // //lidar
  // ////////////////////////////////////////////////////////////////////////

  // lidar enable
  // ros::ServiceClient set_lidar_client;
  // webots_ros::set_int lidar_srv;
  // ros::Subscriber sub_lidar;

  // set_lidar_client = n.serviceClient<webots_ros::set_int>(model_name + "/lidar/enable");
  // lidar_srv.request.value = TIME_STEP;
  // if (set_lidar_client.call(lidar_srv) && lidar_srv.response.success) {
  //   ROS_INFO("Lidar enabled.");
  //   sub_lidar = n.subscribe(model_name + "/lidar/range_image", 1, lidarCallback);
  //   callbackCalled = false;
  //   ROS_INFO("Topic for lidar initialized.");

  //   while (sub_lidar.getNumPublishers() == 0 || !callbackCalled) {
  //     ros::spinOnce();
  //     time_step_client.call(time_step_srv);
  //   }
  //   ROS_INFO("Topic for lidar color connected.");
  // } else {
  //   if (!lidar_srv.response.success)
  //     ROS_ERROR("Sampling period is not valid.");
  //   ROS_ERROR("Failed to enable lidar.");
  //   return 1;
  // }

  // sub_lidar.shutdown();
  // set_lidar_client.shutdown();
  // time_step_client.call(time_step_srv);

  // // lidar_get_info
  // ros::ServiceClient get_info_client = n.serviceClient<webots_ros::lidar_get_info>(model_name + "/lidar/get_info");
  // webots_ros::lidar_get_info get_lidar_info_srv;
  // if (get_info_client.call(get_lidar_info_srv))
  //   ROS_INFO("Lidar of %s has a horizontal resolution of %d, %d layers, a field of view of %f, a min range of %f and a max "
  //            "range of %f.",
  //            model_name.c_str(), get_lidar_info_srv.response.horizontalResolution, get_lidar_info_srv.response.numberOfLayers,
  //            get_lidar_info_srv.response.fov, get_lidar_info_srv.response.minRange, get_lidar_info_srv.response.maxRange);
  // else
  //   ROS_ERROR("Failed to call service lidar_get_info.");

  // get_info_client.shutdown();
  // time_step_client.call(time_step_srv);

  // // lidar_get_frequency_info
  // get_info_client = n.serviceClient<webots_ros::lidar_get_frequency_info>(model_name + "/lidar/get_frequency_info");
  // webots_ros::lidar_get_frequency_info get_lidar_frequency_info_srv;
  // if (get_info_client.call(get_lidar_frequency_info_srv))
  //   ROS_INFO("Lidar %s current frequency is %f, maximum frequency is %f and minimum frequency is %f.", model_name.c_str(),
  //            get_lidar_frequency_info_srv.response.frequency, get_lidar_frequency_info_srv.response.maxFrequency,
  //            get_lidar_frequency_info_srv.response.minFrequency);
  // else
  //   ROS_ERROR("Failed to call service lidar_get_frequency_info.");

  // get_info_client.shutdown();
  // time_step_client.call(time_step_srv);

  // ROS_INFO("Lidar disabled.");
  
  // ////////////////////////////////////////////////////////////////////////
  // //range_finder
  // ////////////////////////////////////////////////////////////////////////

  // // range-finder enable
  // ros::ServiceClient set_range_finder_client;
  // webots_ros::set_int range_finder_srv;
  // ros::Subscriber sub_range_finder_color;

  // set_range_finder_client = n.serviceClient<webots_ros::set_int>(model_name + "/range_finder/enable");
  // range_finder_srv.request.value = TIME_STEP;
  // if (set_range_finder_client.call(range_finder_srv) && range_finder_srv.response.success) {
  //   ROS_INFO("Range-finder enabled.");
  //   sub_range_finder_color = n.subscribe(model_name + "/range_finder/range_image", 1, rangeFinderCallback);
  //   callbackCalled = false;
  //   ROS_INFO("Topic for range-finder initialized.");

  //   while (sub_range_finder_color.getNumPublishers() == 0 || !callbackCalled) {
  //     ros::spinOnce();
  //     time_step_client.call(time_step_srv);
  //   }
  //   ROS_INFO("Topic for range-finder connected.");
  // } else {
  //   if (!range_finder_srv.response.success)
  //     ROS_ERROR("Sampling period is not valid.");
  //   ROS_ERROR("Failed to enable range-finder.");
  //   return 1;
  // }

  // sub_range_finder_color.shutdown();
  // set_range_finder_client.shutdown();
  // time_step_client.call(time_step_srv);

  // // range_finder_get_info
  // get_info_client = n.serviceClient<webots_ros::range_finder_get_info>(model_name + "/range_finder/get_info");
  // webots_ros::range_finder_get_info get_range_finder_info_srv;
  // if (get_info_client.call(get_range_finder_info_srv))
  //   ROS_INFO(
  //     "Range-finder of %s has a width of %d, a height of %d, a field of view of %f, a min range of %f and a max range of %f.",
  //     model_name.c_str(), get_range_finder_info_srv.response.width, get_range_finder_info_srv.response.height,
  //     get_range_finder_info_srv.response.Fov, get_range_finder_info_srv.response.minRange,
  //     get_range_finder_info_srv.response.maxRange);
  // else
  //   ROS_ERROR("Failed to call service range_finder_get_info.");

  // get_info_client.shutdown();
  // time_step_client.call(time_step_srv);

  // // range_finder_save_image
  // ros::ServiceClient save_image_client = n.serviceClient<webots_ros::save_image>(model_name + "/range_finder/save_image");
  // webots_ros::save_image save_range_image_srv;
  // save_range_image_srv.request.filename = std::string(getenv("HOME")) + std::string("/test_image_range_finder.png");
  // save_range_image_srv.request.quality = 100;

  // if (save_image_client.call(save_range_image_srv) && save_range_image_srv.response.success == 1)
  //   ROS_INFO("Image saved.");
  // else
  //   ROS_ERROR("Failed to call service save_image.");

  // save_image_client.shutdown();
  // time_step_client.call(time_step_srv);

  // ROS_INFO("Range-finder disabled.");
}